
#include <ros/ros.h>
#include <math.h>

// Ros
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <image_geometry/stereo_camera_model.h>

// Ros -> openCV -> Ros
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

// PCL
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>

// Image Publishing for Debug
#include <image_transport/image_transport.h>

// Blob Detection
#include <cmvision/Blobs.h>

// Move Head
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/PointHeadGoal.h>
#include <pr2_controllers_msgs/PointHeadAction.h>


using namespace sensor_msgs;
using namespace stereo_msgs;
using namespace message_filters;


class FollowBlobs 
{
private:
	image_transport::Publisher pub;
	image_geometry::StereoCameraModel camModel;
	cmvision::Blob biggestBlob;

public:

	void stereoOnlyCallback(const ImageConstPtr& leftImage, const CameraInfoConstPtr& leftInfo, 
				const ImageConstPtr& rightImage, const CameraInfoConstPtr& rightInfo,
				const DisparityImageConstPtr& disparity)
	{

	

		camModel.fromCameraInfo(leftInfo,rightInfo);
	}


	void blobCallback(const cmvision::BlobsConstPtr& msg)
	{
		if (msg->blob_count == 0) {
			//hasBlob = false;
			return;
		}

		ROS_INFO("Found Blobs:  %i", msg->blob_count);

		// largest blob:
		unsigned int bestIndex = 0;
		for ( unsigned int i = 0; i < msg->blob_count; i++ ) {
			if (msg->blobs[bestIndex].area < msg->blobs[i].area) {
				bestIndex = i;
			}
		}

		biggestBlob = msg->blobs[bestIndex];
		ROS_INFO("\tImage Location:  %ix%i\tarea: %i",biggestBlob.x, biggestBlob.y, biggestBlob.area);
		ROS_INFO("\t\t%i\t%i\t%i\t%i",biggestBlob.right,biggestBlob.left, biggestBlob.bottom, biggestBlob.top);

		//hasBlob = true;
	}

	FollowBlobs() {
		ros::NodeHandle nh;
		message_filters::Subscriber<Image> leftImage(nh, "/wide_stereo/left/image_rect_color", 1);
		message_filters::Subscriber<Image> rightImage(nh, "/wide_stereo/right/image_rect_color", 1);
	
		message_filters::Subscriber<CameraInfo> leftCamInfo(nh, "/wide_stereo/left/camera_info", 1);
		message_filters::Subscriber<CameraInfo> rightCamInfo(nh, "/wide_stereo/right/camera_info", 1);

		message_filters::Subscriber<DisparityImage> disparityImage(nh, "/wide_stereo/disparity", 1);

		TimeSynchronizer<Image, CameraInfo, Image, CameraInfo, DisparityImage> sync(leftImage, leftCamInfo, rightImage, leftCamInfo, disparityImage, 5);
		sync.registerCallback(boost::bind(&FollowBlobs::stereoOnlyCallback,this, _1, _2, _3, _4, _5));

		ros::Subscriber blobDetector = nh.subscribe<cmvision::Blobs> ("blobs",1,FollowBlobs::blobCallback);

		
		//image_transport::ImageTransport it(nh);
		//pub = it.advertise("out_image_base_topic", 1);
		
	}
};

int main(int argc, char** argv) {
	
	ros::init(argc, argv, "sub_pcl");
	FollowBlobs fb();
	ros::spin();
}
